//
// Created by Zemon on 2024/11/2.
//

#include "store.h"
#include "CAN_receive.h"
#include "motor_pid.h"

extern motor_measure_t motor_chassis[5];

pid_t pid_store_speed;
pid_t pid_store_angle;
float kps_s = 8.0f, kis_s = 0.0f, kds_s = 0.0f;
float kpa_s = 0.15f, kia_s = 0.0f, kda_s = 0.0f;
static float set_speed,set_speed_temp;
static int16_t speed_delta;
static int16_t max_speed_change = 30000;

float set_speed_s=0;

void store(float set_angle_s)
{

    /*PID_struct_init(&pid_store_speed,POSITION_PID,16384,16384,kps_s,kis_s,kds_s);
    speed_delta = set_speed_s - motor_chassis[3].speed_rpm;
    if(speed_delta>max_speed_change)
        set_speed_temp = (float)(motor_chassis[3].speed_rpm + max_speed_change);
    else if(speed_delta<-max_speed_change)
        set_speed_temp = (float)(motor_chassis[3].speed_rpm - max_speed_change);
    else
        set_speed_temp = set_speed_s;
    pid_calc(&pid_store_speed,(float)motor_chassis[3].speed_rpm,set_speed_temp);
    CAN_cmd_store((int16_t)pid_store_speed.pos_out);
    */
    PID_struct_init(&pid_store_angle,POSITION_PID,3500,0,kpa_s,kia_s,kda_s);
    PID_struct_init(&pid_store_speed,POSITION_PID,15000,1500,kps_s,kis_s,kds_s);

    pid_calc(&pid_store_angle,(float)motor_chassis[3].total_angle,(float)set_angle_s);
    set_speed = pid_store_angle.pos_out;
    speed_delta = (int16_t)set_speed - motor_chassis[3].speed_rpm;
    if(speed_delta > max_speed_change )
        set_speed_temp = (float)(motor_chassis[3].speed_rpm + max_speed_change);
    else if(speed_delta < -max_speed_change )
        set_speed_temp = (float)(motor_chassis[3].speed_rpm - max_speed_change);
    else
        set_speed_temp = set_speed;
    pid_calc(&pid_store_speed,motor_chassis[3].speed_rpm,set_speed_temp);
    CAN_cmd_store((int16_t)pid_store_speed.pos_out);
}